#ifndef ALGORITHMUR_H
#define ALGORITHMUR_H
#include <array>
#include <eigen3/Eigen/Eigen>
#include <memory>
#include <atomic>
#include "UrParameter.h"
#include "Common/CommonData.h"

using namespace UrParameter;
using namespace Eigen;

namespace CURCONTROL{
class CURControl;
}

using namespace CURCONTROL;

class AlgorithmUR
{
public:
    AlgorithmUR(const std::string &ip);
    void ForwardKinematics(const std::array<double, JointSize> &curJointAngle, std::vector<double> &pos);// 正向运动学
    void InverseKinematics(const std::vector<double> &pos, std::array<double, JointSize> &jointAngle);// 逆运动学
    void MoveTool(xbox_map_t map);
    void MoveTool(double pose[6]);
private:
    // 旋转向量转齐次矩阵
    void RotVec2Mat(const std::vector<double> &pos, MatrixXd &T);
    // 齐次矩阵转旋转向量
    void Mat2RotationVec(const MatrixXd &T, std::vector<double> &pos);
private:
    void InitParameter();// 初始化参数
    void ConnectCallback(bool isConnected);// 机械臂连接回调函数
    // 函数用于4x4矩阵乘法
    void MatrixMultiply4x4(const std::array<std::array<double, 4>, 4> &A,
                           const std::array<std::array<double, 4>, 4> &B,
                           std::array<std::array<double, 4>, 4> &C);
    void Degrees2Rad(const std::array<double, JointSize> &jointAngle, std::array<double, JointSize> &rad);
private:
    UrType type_{UrType::UNKNOWN};
    DH dh_;
    std::shared_ptr<CURControl> robot_;
    std::atomic<double> moveStepx_{0};
    std::atomic<double> moveStepy_{0};
    std::atomic<double> moveStepz_{0};
    std::atomic<double> moveRx_{0};
    std::atomic<double> moveRy_{0};
    std::atomic<double> moveRz_{0};
};

#endif // ALGORITHMUR_H
